package se.webbzon.oschi01.collision3d;

import javax.media.j3d.BoundingBox;
import javax.media.j3d.Transform3D;
import javax.vecmath.Matrix3d;
import javax.vecmath.Point3d;

public class CollisionTriangle3D implements Cloneable {
	
	// An identity transform used for triangles with uninitialized transforms 
	private static final Transform3D identity = new Transform3D();
	
	// A limit used to determine if a matrix can be inverted or not 
	private static final double determinantTolerance = 0.0001;
	
	// The edges of the triangle
	private final Point3d[] edges;
	
	// The boundary of this collision triangle
	private final BoundingBox boundary;
	
	// The transform used to transform the collision triangle
	private Transform3D transform;
	
	/*============================================================
	Constructors
	============================================================*/
	
	/** Creates a new collision triangle from an array of at least 3 points.
	 * The triangle will only copy the first 3 elements of the array. **/
	public CollisionTriangle3D(Point3d[] edges) {
		this(edges[0],edges[1],edges[2]);
	}
	
	public CollisionTriangle3D(Point3d e1, Point3d e2, Point3d e3) {
		edges = new Point3d[3];
		edges[0] = new Point3d(e1);
		edges[1] = new Point3d(e2);
		edges[2] = new Point3d(e3);
		double	xMin = Double.MAX_VALUE, yMin = Double.MAX_VALUE, 
				zMin = Double.MAX_VALUE, xMax = Double.MIN_NORMAL, 
				yMax = Double.MIN_VALUE, zMax = Double.MIN_VALUE;
		for (int i = 0; i < 3; i++) {
			xMin = Math.min(Math.min(Math.min(xMin, edges[0].x),edges[1].x),edges[2].x);
			yMin = Math.min(Math.min(Math.min(yMin, edges[0].y),edges[1].y),edges[2].y);
			zMin = Math.min(Math.min(Math.min(zMin, edges[0].z),edges[1].z),edges[2].z);
			xMax = Math.max(Math.max(Math.max(xMax, edges[0].x),edges[1].x),edges[2].x);
			yMax = Math.max(Math.max(Math.max(yMax, edges[0].y),edges[1].y),edges[2].y);
			zMax = Math.max(Math.max(Math.max(zMax, edges[0].z),edges[1].z),edges[2].z);
		}
		boundary = new BoundingBox(new Point3d(xMin,yMin,zMin),new Point3d(xMax,yMax,zMax));
		transform = identity;
	}
	
	/*============================================================
	Public Methods
	============================================================*/
	
	/** Copies the transformed edges of this CollisionTriangle to the given
	 * array of points. The input array must be at least of size 3. **/
	public void getEdges(Point3d[] edges) {
		transform.transform(this.edges[0],edges[0]);
		transform.transform(this.edges[1],edges[1]);
		transform.transform(this.edges[2],edges[2]);
	}
	
	/** Transforms the collision triangle permanently. **/
	public void transform(Transform3D transform) {
		boundary.transform(transform);
		for (int i = 0; i < 3; i++) {
			transform.transform(edges[i]);
		}
	}
	
	/** Sets the transform associated to this triangle. Any changes
	 * to the transform will change the appearance of the triangle.
	 * The transformed can be shared with other triangles.  **/
	public void setTransform(Transform3D transform) {
		this.transform = transform;
	}
	
	/** Returns true if this triangle intersect the given triangle.
	 * This method uses an already allocated Point3d, Matrix3d and
	 * two BoundingBoxes given by the user. This method should only
	 * be called from a CollisionModel due to optimization. **/
	public boolean intersects(	CollisionTriangle3D triangle, 
								CollisionAllocation3D allocation) {
		// Transform the vertices of the other triangle
		final Point3d[] other = new Point3d[] {
				allocation.getAllocatedPoint(0),
				allocation.getAllocatedPoint(1),
				allocation.getAllocatedPoint(2)
		};
		for (int i = 0; i < 3; i++) 
			triangle.transform.transform(triangle.edges[i],other[i]);
		
		// Get the bounding box of the other triangle
		final double xoMin = Math.min(Math.min(other[0].x,other[1].x),other[2].x);
		final double yoMin = Math.min(Math.min(other[0].y,other[1].y),other[2].y);
		final double zoMin = Math.min(Math.min(other[0].z,other[1].z),other[2].z);
		final double xoMax = Math.max(Math.max(other[0].x,other[1].x),other[2].x);
		final double yoMax = Math.max(Math.max(other[0].y,other[1].y),other[2].y);
		final double zoMax = Math.max(Math.max(other[0].z,other[1].z),other[2].z);

		// Transform the vertices of this triangle
		final Point3d[] self = new Point3d[] {
				allocation.getAllocatedPoint(3),
				allocation.getAllocatedPoint(4),
				allocation.getAllocatedPoint(5)
		};
		for (int i = 0; i < 3; i++) 
			transform.transform(edges[i],self[i]);
		
		// Get the bounding box of this triangle
		final double xsMin = Math.min(Math.min(self[0].x,self[1].x),self[2].x);
		final double ysMin = Math.min(Math.min(self[0].y,self[1].y),self[2].y);
		final double zsMin = Math.min(Math.min(self[0].z,self[1].z),self[2].z);
		final double xsMax = Math.max(Math.max(self[0].x,self[1].x),self[2].x);
		final double ysMax = Math.max(Math.max(self[0].y,self[1].y),self[2].y);
		final double zsMax = Math.max(Math.max(self[0].z,self[1].z),self[2].z);
		
		
		// Check if the bounding boxes intersects
		if (	xsMin > xoMax || xsMax < xoMin ||
				ysMin > yoMax || ysMax < yoMin ||
				zsMin > zoMax || zsMax < zoMin)
			return false;
		else
			return intersectInner(self,other,allocation) ? 
					true : triangle.intersectInner(other,self,allocation);
	}
	
	@Override public Object clone() {
		return new CollisionTriangle3D(edges);
	}
	
	/*============================================================
	Private Methods
	============================================================*/
	
	/** Returns true if this collision vertex intersects the other vertex. **/
	private boolean intersectInner(	Point3d[] self, Point3d[] other,
									CollisionAllocation3D allocation) {
		// Create the matrix
		final Matrix3d ABC = allocation.getAllocatedMatrix(0);
		final Matrix3d ABCi = allocation.getAllocatedMatrix(1);
		ABC.setColumn(1, other[2].x-other[0].x, other[2].y-other[0].y, other[2].z-other[0].z);
		ABC.setColumn(2, other[1].x-other[0].x, other[1].y-other[0].y, other[1].z-other[0].z);
		
		final Point3d d = allocation.getAllocatedPoint(6);
		for (int i = 0; i < 3; i++) {
			// Get the next edge of the vertex
			final int j;
			if (i == 2)
				j = 0;
			else
				j = i+1;
			
			// Calculate the intersection between the line and the plane
			d.set(	self[i].x-other[0].x, 
					self[i].y-other[0].y, 
					self[i].z-other[0].z);
			
			final double[] A = {	self[j].x-self[i].x,
									self[j].y-self[i].y,
									self[j].z-self[i].z};
			ABC.setColumn(0, A);
			// Check if the line is parallel to the plane
			if (Math.abs(ABC.determinant()) > determinantTolerance) {
				// Orthogonal projection the plane containing the vertex
				ABCi.invert(ABC);
				ABCi.transform(d);
			
				// Check if the intersection is inside the vertex
				if (d.x <= 0 && d.x >= -1 && 0 <= d.y && d.y <= 1 && 0 <= d.z && d.z <= 1-d.y)
					return true;
			}
		}
		return false;
	}

}
